#include "task_main.h"
#include "mpu6050.h"
#include "HMC5883L.h"
#include "Car.h"

#define TASK_BUFFER_SIZE 12
stimer_task_t task_buffer[TASK_BUFFER_SIZE];
uint16_t TaskInputID, TaskOutputID, TaskMsgHandleID, TaskDisplayID;
#define debugPrint(str, ...) UARTx_Printf(&huart1, str, ##__VA_ARGS__)

void TaskInput(void const *argument);
void TaskOutput(void const *argument);
void TaskDisplay(void const *argument);
void Task_MsgHandle(void const *argument);

void user_task_init(void)
{
    stimer_init(task_buffer, TASK_BUFFER_SIZE);

    TaskInputID = stimer_create_task(TaskInput, 10, 1, 1);
    TaskOutputID = stimer_create_task(TaskOutput, 1, 2, 1);
    TaskDisplayID = stimer_create_task(TaskDisplay, 300, 0, 1);
    TaskBuzzerID = stimer_create_task(TaskBuzzer, 50, 1, 1);
}

void Main(void)
{
    /* USER CODE BEGIN Main */
	board_base_init();
    user_task_init();
    Beep(200);

    // stimer_task_oneshot(task_connect_wifi_start, 3000, 1, NULL);
    //    HAL_TIM_Base_Start(&htim15);
    stimer_task_delay_start(TaskInputID, STIMER_TASK_LOOP, (void*)"TaskInput", 0);
    stimer_task_delay_start(TaskOutputID, STIMER_TASK_LOOP, (void*)"TaskOutput", 5);
    // stimer_task_delay_start(TasK_, STIMER_TASK_LOOP, (void*)"TaskMsgHandle", 10);
    stimer_task_delay_start(TaskDisplayID, STIMER_TASK_LOOP, (void*)"TaskDisplay", 2000);
    MPU6050_Init(Sensor_I2C2_Serch());
	Init_QMC5883();
	Display_Init();

    /* Infinite loop */
    for (;;)
    {
        stimer_serve();
        Task_MsgHandle(0);
        TaskDisplay(0);
        // HAL_Delay(300);
    }
    /* USER CODE END Main */
}

/**
 * @brief 积分计算陀螺仪角度
 * @param data 
 * @retval float 
 */
float calculateAngles(MPU6050_t *data)
{
    static float gyroAngleZ = 0;
    static float caliVal = 0;
    static uint16_t calicnt = 0;
    static uint8_t califlag = 0;
    // 陀螺仪的原始数据
    float gyroZ = data->Gyro_Z;
    // 假设采样周期为0.01秒
    if (califlag == 0)
    {
        gyroAngleZ += gyroZ;
        calicnt++;
        if (calicnt >= 100) // 开机矫正100个点
        {
            caliVal = gyroAngleZ/100;
            gyroAngleZ = 0;
            califlag = 1;
        }
		return 0;
    }
    else
    {
        gyroAngleZ += (gyroZ-caliVal)*0.01;//  * 180 / 3.14159265358979323846
		return gyroAngleZ;
    }
    
}
/**
 * @brief 传感器扫描任务
 * @param argument
 */
void Task_SensorScan(void const *argument)
{
    /* USER CODE BEGIN Task_SensorScan */

    /* Infinite loop */
    Car_CheckPower();
    MPU6050_Read_Accel();
    MPU6050_Read_Gyro();
	car.angle = calculateAngles(&mpu6050);
//    car.angle = Multiple_Read_QMC5883();
    WS_Car_Get_Track_Date();
    car_ctrl_angle_loop(); // 角度闭环
    /* USER CODE END Task_SensorScan */
}

/**
 * @brief 板载资源任务
 * @param argument
 * @date 2022-11-07
 */
void TaskInput(void const *argument)
{
    /* USER CODE BEGIN Task_BoardBase */
    static uint32_t cnt = 0;
    uint8_t key_input[BSP_KEY_NUM];
    /* Infinite loop */
    bsp_key_input_raed(key_input, BSP_KEY_NUM);
    if (iSW_Scan(hisw1, key_input, BSP_KEY_NUM))
    {
        Beep(40);
        /* [5] 按键事件处理 */
        if (hisw1[0].events & iSW_EVENT_CLICK)
        {
			debugPrint("KEY0 click\n\r");
        }
        if (hisw1[1].events & iSW_EVENT_CLICK)
        {
            if(car.mode == CARMode_Patrol) Car_Patrol_Stop();
            else Car_Patrol_Start();
            car.DisplayEN = 1;
			debugPrint("KEY1 click\n\r");
        }
        if (hisw1[2].events & iSW_EVENT_CLICK)
        {
			car_ctrl_stop();
			CarCtrl.port->buzzer_write(0);
			debugPrint("KEY2 click\n\r");
        }

    }
    iSW_Clear(&hisw1[0], BSP_KEY_NUM);

    if (cnt++ > 50)
    {
        cnt = 0;
        LED_Run(0);//500ms
    }
    Task_SensorScan(0);
    /* USER CODE END Task_BoardBase */
}

/**
 * @brief 消息处理任务
 * @param argument
 */
void Task_MsgHandle(void const *argument)
{
    /* USER CODE BEGIN Task_MsgHandle */
    uint8_t *pdata = NULL;
    UART_HandleTypeDef *phuart = NULL;
    uint32_t size = 0;
	(void)phuart;
    /* Infinite loop */
    // 获取任务通知值并清空
    if (hrx1.rxCpltFlag) /* 读到了数据 */
    {
        hrx1.rxCpltFlag = 0;
        phuart = &huart1;
        pdata = UART_GetRxData(&hrx1);
        size = UART_GetRxSize(&hrx1);

        // UARTx_Printn(&huart3, (uint8_t *)"[RX1]", 5);
        // UARTx_Printn(&huart3, pdata, size);
        // UARTx_Printn(&huart3, (uint8_t *)"\n\r", 2);
        // UARTx_Printn(&huart2, pdata, size);
    }
    else if (hrx2.rxCpltFlag) /* 读到了数据 */
    {
        hrx2.rxCpltFlag = 0;
        phuart = &huart2;
        pdata = UART_GetRxData(&hrx2);
        size = UART_GetRxSize(&hrx2);
        if (size)
        {
            CarCtrl_CMD(pdata, size);
        }
    }
    
    /* USER CODE END Task_MsgHandle */
}


/**
 * @brief 输出处理任务
 * @param argument
 * @date 2022-11-07
 */
void TaskOutput(void const *argument)
{
    /* USER CODE BEGIN Task_OutputHandle */
    /* Infinite loop */
    car_ctrl_track_loop();

    /* USER CODE END Task_OutputHandle */
}


char *tracker_print(uint16_t data)
{
    static char tracker_temp_buff[17]={0};
    for (size_t i = 0; i < 16; i++)
    {
        tracker_temp_buff[i] = (data&1) ? 'x' : 'o';
        data >>= 1;
    }
	return &tracker_temp_buff[0];
}
  
//u8g2_t u8g2;
void Display_Init(void)
{
//	u8g2Init(&u8g2);
//    u8g2_FirstPage(&u8g2);
//    do
//    {
//        draw_test(&u8g2);

//    } while (u8g2_NextPage(&u8g2));
//    u8g2_SetFontMode(&u8g2, 1);            /*字体模式选择*/
//    u8g2_SetFontDirection(&u8g2, 0);       /*字体方向选择*/
//    u8g2_SetFont(&u8g2, u8g2_font_5x7_tf); /*字库选择*/
    OLED_Init();
}
/**
* @brief 显示任务
* @param argument: Not used
* @retval None
*/
void TaskDisplay(void const * argument)
{
  /* USER CODE BEGIN TaskDisplay */

  /* Infinite loop */

//    while(car.DisplayEN == 0) return;
//    u8g2_FirstPage(&u8g2);
//    do
//    {
//        u8g2_DrawStr(&u8g2, 00, 7, FormatToStr("LEncV:%.2f", MotorL.v));
//        u8g2_DrawStr(&u8g2, 60, 7, FormatToStr("REncV:%.2f", MotorR.v));
//        u8g2_DrawStr(&u8g2, 00, 15, FormatToStr("LEnc:%ld", MotorL.encoder));
//        u8g2_DrawStr(&u8g2, 60, 15, FormatToStr("REnc:%ld", MotorR.encoder));
//        u8g2_DrawStr(&u8g2, 00, 23, FormatToStr("V:%.3f    Tr:%X", car.powerV, traceData.data));
//        u8g2_DrawStr(&u8g2, 00, 31, FormatToStr("[%.1f,%.1f,%.1f]  ", \
//                    mpu6050.Gyro_X, mpu6050.Gyro_Y, mpu6050.Gyro_Z));
//        u8g2_DrawStr(&u8g2, 00, 39, FormatToStr("PIDK[%.0f,%.0f,%.0f]",car.PID_P,car.PID_I,car.PID_D));
//        u8g2_DrawStr(&u8g2, 00, 47, FormatToStr("V:%d,M:%d  T:%.2f",\
//                    car.baseSpeed,car.mode,((float)HAL_GetTick() - car.PatrolTimeCounter)/1000));
//        u8g2_DrawStr(&u8g2, 00, 55, FormatToStr("%s",tracker_print(traceData.data)));

//        // if(MotorL.pidEN && MotorR.pidEN)
    // UARTx_Printf(&huart1,"Speed:%.2f,%.2f,%.2f,%.2f\n\r",
    //              CarCtrl.fb_LSpeed, CarCtrl.fb_RSpeed, CarCtrl.real_LSpeed, CarCtrl.real_RSpeed);
    // UARTx_Printf(&huart1,"Angle:%.2f,%.2f,%.2f\n\r",
    //              CarCtrl.fb_angle, CarCtrl.out_LSpeed, CarCtrl.out_RSpeed);
    UARTx_Printf(&huart1,"mpu:%.2f,%.2f,%.2f\n\r", car.angle, mpu6050.Accel_Z, mpu6050.Gyro_Z);
//       UARTx_Printf(&huart1,"mpu:%.2f,%.2f,%f,%.2f\n\r", CarCtrl.fb_angle, CarCtrl.real_angle, mpu6050.Gyro_Z, CarCtrl.real_LSpeed);
//    } while (u8g2_NextPage(&u8g2));
//    
    /* USER CODE END TaskDisplay */
}

/**
  * @brief  Period elapsed callback in non blocking mode
  * @note   This function is called  when TIM1 interrupt took place, inside
  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
  * a global variable "uwTick" used as application time base.
  * @param  htim : TIM handle
  * @retval None
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  /* USER CODE BEGIN Callback 0 */

  /* USER CODE END Callback 0 */
  /* USER CODE BEGIN Callback 1 */
	if(htim->Instance == TIM2)
	{
        static uint8_t TCnt = 0;
        TCnt++;
        if(TCnt >= 120) // 1200Hz/120 = 10Hz
        {
            TCnt = 0;
            User_TIM_PECallback(htim); // 编码器速度计算
            car_ctrl_speed_loop(); // 速度环
        }
	}
  /* USER CODE END Callback 1 */
}

